#pragma once
#ifndef PRM_H
#define PRM_H

#include "basic_typedef.h"
#include "Graph.h"
#include "CollisionDetector.h"
#include "Sampler.h"
#include "Kd_tree_d.h"
#include "Clearance_detector.hpp"

#include <Windows.h>
#include <vector>
// Should be used with the Graph wrapper
struct Less_than_int
{
  bool operator()(const int& p1, 
                  const int& p2) const 
  {
    return (p1 < p2);
  }
};

class Prm
{
public:
	/*
		IMPORTANT:	The main code of the PRM algorithm should be placed in this file.
					You may change its structure, but make sure you that it remains consistent
					with the run() operation in Planner.h
	*/


	Prm (	int number_vertices, 
			int k_nearest, 
			CollisionDetector* col, 
			MultiRobotsSampler* sampler,
			ConfSet start, ConfSet target, bool use_distance);

	Prm();

	void add_vertices(UINT32 number_vertices);

	~Prm();
	
	//	This operation is supposed to generate the roadmap (sample configurations
	//	and connect them.
	//	TODO: implement it.
	void			generate_roadmap();

	void add_path(const std::vector<ConfSet>& new_path);
	void add_connections(const std::vector<std::vector<ConfSet>>& points_to_add);

	//	Returns a point path from start to target.
	//	If a path doesn't exist, returns empty vector.
	//	TODO: Implement this function.
	std::vector<ConfSet> retrieve_path();
	void generate_path();
	double get_path_quality();
	bool has_path();
	bool is_null();
	void set_clearance_detector(Clearance_detector_ie* detector1,Clearance_detector_ie* detector2,double alpha);

private:
	void add_point_to_graph(Point_d p, int index);
	double calculate_edge_weight(ConfSet p,ConfSet q);
	void add_start_and_target_to_graph();
	void add_start_and_target();
	int get_point_index(Point_d p);
	ConfSet get_conf_set(Point_d point);
	Point_d get_point_d(ConfSet c) const;


	int									m_number_vertices;	// number of sampled vertices
	int									m_k_nearest;		// maximal degree of vertex in roadmap
	CollisionDetector*					m_col;
	MultiRobotsSampler*					m_sampler;
	UINT32								_start_index;
	UINT32								_end_index;
	
	Graph<int>*							m_graph;			//	Graph structure
	Kd_tree_d<Kernel_d>*				m_kd_tree;			//	Kd-tree for nearest neighbor search

	ConfSet 							m_start,m_target;	//	Start and target configurations
	vector<Point_d>						m_all_tree_points;
	vector<ConfSet>						_path;
	bool								_null_prm;
	bool								_use_distance;
	Clearance_detector_ie*				_clearance_detector_r1_pointer;	
	Clearance_detector_ie*				_clearance_detector_r2_pointer;
	double								_alpha;
	double								_path_quality;

private:
	static const double LOCAL_PLANNER_STEP_SIZE;

};

#endif